#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import os

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import (
    LaunchConfiguration,
    PathJoinSubstitution,
    Command,
    FindExecutable,
)
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    # 参数：是否使用 mock component
    use_mock_component = LaunchConfiguration("use_mock_component")

    declare_use_mock_component_arg = DeclareLaunchArgument(
        "use_mock_component",
        default_value="true",
        description="Use mock components in xacro (use_mock_component:=true/false)",
    )

    # 路径：URDF xacro & RViz 配置
    urdf_path = PathJoinSubstitution(
        [FindPackageShare("zc4model"), "urdf", "my_robot.urdf.xacro"]
    )
    rviz_config_path = PathJoinSubstitution(
        [FindPackageShare("zc4model"), "rviz", "zc4_rviz_config.rviz"]
    )

    # 通过 xacro 生成 robot_description
    robot_description = {
        "robot_description": Command(
            [
                FindExecutable(name="xacro"),
                " ",
                urdf_path,
                " ",
                "use_mock_component:=",
                use_mock_component,
            ]
        )
    }

    # 1. robot_state_publisher
    robot_state_publisher_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="screen",
        parameters=[robot_description],
    )

    # 2. ros2_control_node，使用 MoveIt 配置包里的 ros2_controllers.yaml
    ros2_controllers_yaml = PathJoinSubstitution(
        [FindPackageShare("zc4_moveit_config"), "config", "ros2_controllers.yaml"]
    )

    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[ros2_controllers_yaml],
        output="screen",
    )

    # 3. spawner：joint_state_broadcaster
    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_state_broadcaster"],
        output="screen",
    )

    # 4. spawner：joint_trajectory_controller
    joint_trajectory_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_trajectory_controller"],
        output="screen",
    )

    # 5. spawner：arm_position_holder --inactive
    arm_position_holder_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["arm_position_holder", "--inactive"],
        output="screen",
    )

    # 6. 自动回零节点：py_pkg/auto_home
    auto_home_node = Node(
        package="py_pkg",
        executable="auto_home",
        name="auto_home",
        output="screen",
        parameters=[
            {
                "controller_name": "joint_trajectory_controller",
                # 这里用 Python 列表，等价于 XML 中的 YAML 列表
                "joint_names": ["joint11", "joint2", "joint3", "joint4", "joint5"],
                "target_positions": [0.0, 1.0472, -2.0944, 1.0472, 0.0],
                "duration_sec": 6.0,
            }
        ],
    )

    # 7. 引用 MoveIt 的 move_group.launch.py
    move_group_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution(
                [FindPackageShare("zc4_moveit_config"), "launch", "move_group.launch.py"]
            )
        )
    )

    # 8. RViz2
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
        arguments=["-d", rviz_config_path],
    )

    return LaunchDescription(
        [
            declare_use_mock_component_arg,
            robot_state_publisher_node,
            ros2_control_node,
            joint_state_broadcaster_spawner,
            joint_trajectory_controller_spawner,
            arm_position_holder_spawner,
            auto_home_node,
            move_group_launch,
            rviz_node,
        ]
    )
